﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Diagnostics;
using System.Drawing;
using System.Threading;
using System.Windows.Forms;
using System.Xml;
using AForge.Imaging.Filters;
using Microsoft.Ccr.Adapters.WinForms;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Services.MountService;
using Microsoft.Dss.Services.Serializer;
using Microsoft.Robotics.PhysicalModel;
using Microsoft.Robotics.Services.IRobot.Roomba.Proxy;
using Microsoft.Robotics.Services.IRobot.SensorUpdates.Proxy;
using Microsoft.Robotics.Services.Sensors.Kinect.Proxy;
using Microsoft.Robotics.Simulation.Engine;
using Microsoft.Robotics.Simulation.Physics;
using NestedParticleFilter;
using Microsoft.Robotics.Services.DepthCamSensor;
using W3C.Soap;
using kinect = Microsoft.Robotics.Services.Sensors.Kinect.Proxy;
using depthcamsensor = Microsoft.Robotics.Services.DepthCamSensor.Proxy;
using webcam = Microsoft.Robotics.Services.WebCamSensor.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using engine = Microsoft.Robotics.Simulation.Engine.Proxy;
using lite = Microsoft.Robotics.Services.IRobot.Lite.Proxy;
using sensorupdates = Microsoft.Robotics.Services.IRobot.SensorUpdates.Proxy;
using mount = Microsoft.Dss.Services.MountService;
using Point = NestedParticleFilter.Point;

namespace iCreateSimulation
{
    [Contract(Contract.Identifier)]
    [DisplayName("iCreateSimulation")]
    [Description("iCreateSimulation service (no description provided)")]
    partial class iCreateSimulationService : DsspServiceBase
    {
        /// <summary>
        /// Service state
        /// </summary>
        [ServiceState]
        iCreateSimulationState _state = new iCreateSimulationState();

        /// <summary>
        /// Main service port
        /// </summary>
        [ServicePort("/iCreateSimulation", AllowMultipleInstances = true)]
        iCreateSimulationOperations _mainPort = new iCreateSimulationOperations();


        #region Partners

        [Partner("SimulationEngine", Contract = engine.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        engine.SimulationEnginePort _simEngine = new engine.SimulationEnginePort();

        /// <summary>
        /// iCreate partner
        /// </summary>
        [Partner("iCreate", Contract = lite.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        lite.IRobotLiteOperations _iCreatePort = new lite.IRobotLiteOperations();
        lite.IRobotLiteOperations _iCreateNotify = new lite.IRobotLiteOperations();

        /// <summary>
        /// jCreate partner
        /// </summary>
        [Partner("jCreate", Contract = lite.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        lite.IRobotLiteOperations _jCreatePort = new lite.IRobotLiteOperations();
        lite.IRobotLiteOperations _jCreateNotify = new lite.IRobotLiteOperations();



        #region iKinect
        /// <summary>
        /// KinectService partner
        /// </summary>
        [Partner("iKinectService", Contract = kinect.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        KinectOperations _iKinectPort = new kinect.KinectOperations();

        [Partner("iDepthCamService", Contract = depthcamsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        depthcamsensor.DepthCamSensorOperationsPort _iDepthSensor = new depthcamsensor.DepthCamSensorOperationsPort();
        depthcamsensor.DepthCamSensorOperationsPort _iDepthNotifyPort = new depthcamsensor.DepthCamSensorOperationsPort();

        [Partner("iWebCamService", Contract = webcam.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        webcam.WebCamSensorOperations _iWebcamPort = new webcam.WebCamSensorOperations();
        webcam.WebCamSensorOperations _iWebcamNotify = new webcam.WebCamSensorOperations();
        #endregion

        #region jKinect
        /// <summary>
        /// KinectService partner
        /// </summary>
        [Partner("jKinectService", Contract = kinect.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        KinectOperations _jKinectPort = new kinect.KinectOperations();

        [Partner("jDepthCamService", Contract = depthcamsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        depthcamsensor.DepthCamSensorOperationsPort _jDepthSensor = new depthcamsensor.DepthCamSensorOperationsPort();
        depthcamsensor.DepthCamSensorOperationsPort _jDepthNotifyPort = new depthcamsensor.DepthCamSensorOperationsPort();

        [Partner("jWebCamService", Contract = webcam.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        webcam.WebCamSensorOperations _jWebcamPort = new webcam.WebCamSensorOperations();
        webcam.WebCamSensorOperations _jWebcamNotify = new webcam.WebCamSensorOperations();

        #endregion

        #endregion

        #region Constants

        private const String path =
           "C:\\Users\\Anousha\\Microsoft Robotics Dev Studio 4 Beta 2\\Projects\\Environment\\";

        public const int SimulationQueryInterval = 10; // msec
        private const double MapWidth = 30; // in meters
        private const double MapHeight = 23; // in meters
        private const double PatInitialTheta = 180;
        private const double MatInitialTheta = 90;
        private const int UpdateMapInterval = 500; //milliseconds

        private const int KinectPollingInterval = 1000; // polling interval for kinect
        private const int PollQueueForSampling = 0; // in milliseconds
        private const int SamplingInterval = 2; // in second
        private const double JActionTime = 2000; // in milliseconds
        private const double TaggedDistance = 1600;
        private const int QueueLimit = 2;
        private const int Level = 1;
        #endregion

        #region Members
        private Port<bool> _iDriveCompleted, _jDriveCompleted;
        public static int IForwardVelocity = 190; // mm/sec
        public static int JForwardVelocity = 100;
        public static int JPushVelocity = 300;
        public static int NumOfIParticles = 500;
        public static int NumOfJParticles = 40;
        private ImageForm _form;
        private HistogramForm _histogramForm;
        private int _numOfObservation;
        private Map _iMap;
        private Map _jMap;
        private VectorFieldHistogram _vfh;
        private List<Landmark> _mapLandmarks;
        private List<Landmark> _iBeleivedLandmarks;
        private List<Particle> _patParticles;
        private RobotPose _patAveragedPose;
        public static Particle _mostLikelyParticle;
        private static Particle _mostLikelyJParticle;
        private RobotPose _clusteredPose;

        private Queue<Parameter> _patActions;
        private List<NestedParticleFilter.Point> _wayPoints;
        // specification of observed landmarks so far. The color is the landmarks real
        // color and not the one the robot gets from its camera
        private List<CustomisedColor> _observedEntityColor;
        private bool _pushedLandmark;
        private bool _reachedLandmark;  // for setting if j reached the landmark
        private bool _facedTheObject;   // for setting if j is faced the landmark
        private bool _jTagged;          // for setting if j got tagged
        private Cluster _cluster;       // for clustering the particles of i. I am not using this... since average pose gives better result
        private bool _queueExceededLimit;   // if queueing actions is exceeded the queue limit
        private double _meanError;
        private Point[] landmarkPoses;  // for updating the map 
        private int[] count;  // for getting the average over the updated map
        public static DateTime _startTime; // for measuring how long a run is taken
        public static DateTime _taggedTime; // for setting the time where j got tagged
        private Point bluelm;
        private bool _runningExplorer;
        #endregion
        /// <summary>
        /// Service constructor
        /// </summary>
        public iCreateSimulationService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        /// <summary>
        /// Service start
        /// </summary>
        protected override void Start()
        {

            base.Start();

            // initialize the service parameters (particles, forms ...)
            if (!Initialization())
                return;

            Activate
                (Arbiter.Interleave(
                    new TeardownReceiverGroup(),

                new ExclusiveReceiverGroup
                    (
                         Arbiter.ReceiveWithIterator<SimulationReset>(true, _mainPort, SimulationResetHandler)
                    ),
                new ConcurrentReceiverGroup(
                    Arbiter.ReceiveWithIterator<UpdateBumpsCliffsAndWalls>(true, _iCreateNotify, iUpdateBumpsCliffs),
                    Arbiter.ReceiveWithIterator<UpdateBumpsCliffsAndWalls>(true, _jCreateNotify, jUpdateBumpsCliffs),
                    Arbiter.Receive<webcam.Replace>(true, _iWebcamNotify, iWebcamReplaceHandler),
                    Arbiter.Receive<webcam.Replace>(true, _jWebcamNotify, jWebcamReplaceHandler),
                    Arbiter.Receive<depthcamsensor.Replace>(true, _iDepthNotifyPort, iDepthcamReplaceHandler),
                    Arbiter.Receive<depthcamsensor.Replace>(true, _jDepthNotifyPort, jDepthcamReplaceHandler)

                  )
                ));


            Activate<ITask>(
                Arbiter.ReceiveWithIterator<PatExplore>(true, _mainPort, PatExploreHandler),
                Arbiter.ReceiveWithIterator<MatExplore>(true, _mainPort, MatExploreHandler)
                //Arbiter.ReceiveWithIterator<Sample>(false, _mainPort, SampleParticles) 
                );// Arbiter.ReceiveWithIterator<UpdateMap>(true, _mainPort, UpdateMapHandler));
            //Console.WriteLine("Starting the service");
            for (int i = 5; i > 0; i--)
            {
                Debug.Write(i + "  ");
                Thread.Sleep(500);
            }
            UpdateMapHandler();
            SpawnIterator(QueryRobotPosition);
            SpawnIterator(SubscribeToLite);
            SpawnIterator(SubscribeToDepthcam);
            Activate(Arbiter.FromIteratorHandler(SubscribeToWebcam));
            Thread.Sleep(500);
            Activate(Arbiter.FromIteratorHandler(SampleParticles));
            _mainPort.Post(new UpdateMap());
            _meanError = Particle.MeanError(_patParticles, _state.PatPose);
            Console.WriteLine(_meanError);
            //_mainPort.Post(new Sample());

        }

        /// <summary>
        /// initialize the components for irobotlocalization
        /// </summary>
        /// <returns>true if succeeded and false otherwise</returns>
        public bool Initialization()
        {
            bool status = true;
            // isSimulated = false;
            _iMap = new Map(MapWidth, MapHeight, Color.Green, path + "boyd.config.xml"); // map object to represent the particles
            _jMap = new Map(MapWidth, MapHeight, Color.OrangeRed, path + "boyd.config.xml"); // map object to represent the particles
            _vfh = new VectorFieldHistogram(MapWidth, MapHeight);
            _wayPoints = new List<Point>();
            _wayPoints.Add(new Point(9.5, 6.9));
            //_wayPoints.Add(new Point(8.9, -5.5));
            //_wayPoints.Add(new Point(6.9, 7.5));
            //_wayPoints.Add(new Point(8.5,7.5));
            _wayPoints.Add(new Point(11.5, 7.5));
            _wayPoints.Add(new Point(10, 7.5));
            _wayPoints.Add(new Point(9.5, 7.3));
                _wayPoints.Add(new Point(9, 7.2));
            _wayPoints.Add(new Point(8.5, 7.0));
            //_wayPoints.Add(new Point(10, 7.5));
            _wayPoints.Add(new Point(8, 7.0));
            _wayPoints.Add(new Point(7.5, 6.9));
            _wayPoints.Add(new Point(4, 6.5));
           // _wayPoints.Add(new Point(7.5, 6.9));
            _wayPoints.Add(new Point(3, 6.9));
            _patParticles = new List<Particle>(); // uboo particles
            _patParticles = Particle.NonUniformParticleInitialization(NumOfIParticles, PatInitialTheta,
                                                                       NumOfJParticles, MatInitialTheta, 0.75, 0.25);
            _patAveragedPose = new RobotPose();
            _clusteredPose = new RobotPose();
            _iDriveCompleted = new Port<bool>();
            _jDriveCompleted = new Port<bool>();
            _numOfObservation = 0;
            _jTagged = false;
            _pushedLandmark = false;
            _facedTheObject = false;
            _reachedLandmark = false;
            _queueExceededLimit = false;
            _runningExplorer = false;
            _patActions = new Queue<Parameter>();
            _mapLandmarks = Landmark.ReadBelievedLandmarks(path + "landmarks.txt"); // populate the list by the landmarks information in landmarks.txt file
            _iBeleivedLandmarks = Landmark.ReadBelievedLandmarks(path + "landmarks.txt");
            _observedEntityColor = new List<CustomisedColor>();
            _meanError = 0.0;
            _cluster = new Cluster(2);
            //_previousOmega = 0;
          
           bluelm = new Point(0,0);
            if (_mapLandmarks == null)
            {
                Debug.WriteLine("Could not find the landmarks.txt file or populate the list");
                status = false;
            }
            else
            {
                landmarkPoses = new Point[_mapLandmarks.Count];
                count = new int[_mapLandmarks.Count]; // for getting the average
                for (int i = 0; i < landmarkPoses.Length; i++)
                {
                    landmarkPoses[i] = new Point(0, 0);
                    count[i] = 0;

                }
            }
           
            // making the forms (map and vfh)
            if (!MakeMapForm())
                status = false;
            if (!MakeHistogramForm())
                status = false;
            return status;
        }

        /*****************************************************************************************/

        #region Robot Handlers

        #region Pats's Sensor Handlers

        /// <summary>
        /// Handler for when the bumper is pressed
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        private IEnumerator<ITask> iUpdateBumpsCliffs(sensorupdates.UpdateBumpsCliffsAndWalls update)
        {
           
            {
                TimeSpan ts = DateTime.UtcNow - _state.LastPressed;
               // Console.WriteLine("Ouch!!!!");
                if (ts.TotalMilliseconds <5000)
                {
                    _state.PatBumperPressed = false;
                    yield break;
                }
                else if ((update.Body.BumpsWheeldrops & (BumpsWheeldrops.BumpLeft | BumpsWheeldrops.BumpRight)) != 0)
                {
                    //Debug.WriteLine("Ouch!!!!");
                    _state.PatBumperPressed = true;
                    _state.LastPressed = DateTime.UtcNow;
                    //Console.WriteLine("****** Ouch");
                    //----------------------------------------------------------------------------/
                    // stop and queue the actions for previous action
                    //Debug.WriteLine("Stopping");
                    SpawnIterator(()=>StopRobot(_iCreatePort, _iDriveCompleted));
                    yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
                    yield return new IterativeTask(QueueActions);
                    //Debug.WriteLine("Stoped");
                    yield return TimeoutPort(1000).Receive();
                    //----------------------------------------------------------------------------/
                    // backout using goStraight function. it takes velocity and time. This function
                    // queue the action so no need to call queue action here
                    yield return new IterativeTask(() => GoStraight(-200, 2000.0, _iCreatePort, _iDriveCompleted, true));
                    //Debug.WriteLine("backout");
                    yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
                    //Debug.WriteLine("backouted");
                    yield return new IterativeTask(() => StopRobot(_iCreatePort, _iDriveCompleted));
                    yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
                    //----------------------------------------------------------------------------/
                    //// rotate using rotateRight function. this will queue the action so no need to 
                    //// call QueueAction specifically here.
                    //Debug.WriteLine("Rotating");
                   // if (rand.Next() % 2 == 0)
                    {
                        yield return TimeoutPort(1000).Receive();
                        yield return
                            new IterativeTask(
                                () => RotateRight(90, IForwardVelocity, _iCreatePort, _iDriveCompleted, true));
                        yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
                    }
                    //else
                    //{
                    //    yield return
                    //        new IterativeTask(
                    //            () => RotateLeft(90, IForwardVelocity, _iCreatePort, _iDriveCompleted, true));
                    //    yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
                    //}
                //----------------------------------------------------------------------------/
                    _state.PatBumperPressed = false;
                    _mainPort.Post(new PatExplore());
                    
                }
               
            }
        }

        /// <summary>
        /// A handler for getting depth information of the kinect from i's robot. This handler updates
        /// the depth image of i in the form.
        /// </summary>
        /// <param name="replace">deoth information</param>
        /// <returns></returns>
        public void iDepthcamReplaceHandler(depthcamsensor.Replace replace)
        {
            DepthCamSensorState depthInfo = replace.Body;
            short[] depthimage = depthInfo.DepthImage; // arrays of short, representing distance in millemeters
            int height = depthInfo.DepthImageSize.Height;
            int width = depthInfo.DepthImageSize.Width;
            //Debug.WriteLine("Depth height; "+ height + " width: "+ width);
            Bitmap image = null;
            if (depthimage != null && _form != null)
            {
                image = ImageProcessing.MakeDepthBitmap(width, height, depthimage);
                //resize the bitmap for display purposes.
                if (image != null)
                {
                    ResizeBilinear filter = new ResizeBilinear(160, 120);
                    _form.SetIDepthPic(filter.Apply(image));
                }

            }
        }

        /// <summary>
        /// A handler for webcam notifications of the kinect for robot I. This handler updates the bitmap
        /// of the robot i in the form.
        /// </summary>
        /// <param name="replaceMessage"></param>
        private void iWebcamReplaceHandler(webcam.Replace replaceMessage)
        {
            var webcamState = replaceMessage.Body;
            try
            {
                if (_form != null && webcamState != null && webcamState.Bitmap != null)
                {
                    _state.KinectBitmap = webcamState.Bitmap;
                    _form.SetIKinectPic(webcamState.Bitmap);
                }

            }
            catch (Exception e)
            {
                Debug.WriteLine("Webcam replace handler: " + e.ToString());
            }

        }
        #endregion

        #region Mat's Sensor Handler

        /// <summary>
        /// Handler for when the bumper is pressed
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        private IEnumerator<ITask> jUpdateBumpsCliffs(sensorupdates.UpdateBumpsCliffsAndWalls update)
        {

            if (!_jTagged || _pushedLandmark)
            {
                if ((update.Body.BumpsWheeldrops & (BumpsWheeldrops.BumpLeft | BumpsWheeldrops.BumpRight)) != 0)
                {
                    //Debug.WriteLine("Ouch!!!!");
                    _state.MatBumperPressed = true;
                    //Debug.WriteLine("Stopping J");
                    yield return new IterativeTask(() => StopRobot(_jCreatePort, _jDriveCompleted));
                    yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                    //Debug.WriteLine("Stoped");
                    yield return TimeoutPort(1000).Receive();
                    //----------------------------------------------------------------------------/
                    // backout using goStraight function. it takes velocity and time. This function
                    // queue the action so no need to call queue action here
                    yield return new IterativeTask(() => GoStraight(-150, 1500.0, _jCreatePort, _jDriveCompleted, false));
                   // Debug.WriteLine("backout");
                    yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                   // Debug.WriteLine("backouted");

                    yield return TimeoutPort(1000).Receive();
                    //----------------------------------------------------------------------------/
                    // rotate using rotateRight function. this will queue the action so no need to 
                    // call QueueAction specifically here.
                    //Debug.WriteLine("Rotating");
                    yield return new IterativeTask(() => RotateRight(90, JForwardVelocity, _jCreatePort, _jDriveCompleted, false));
                    yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                    //----------------------------------------------------------------------------/
                    _state.MatBumperPressed = false;
                    _mainPort.Post(new MatExplore());

                }
            }
            else if (_jTagged && !_reachedLandmark)
            {
                _reachedLandmark = true;

            }
        }


        /// <summary>
        /// A handler for getting depth information of the kinect from Robot J. This handler updates
        /// the depth image of j in the form.
        /// </summary>
        /// <param name="replace">deoth information</param>
        /// <returns></returns>
        public void jDepthcamReplaceHandler(depthcamsensor.Replace replace)
        {
            DepthCamSensorState depthInfo = replace.Body;
            short[] depthimage = depthInfo.DepthImage; // arrays of short, representing distance in millemeters
            int height = depthInfo.DepthImageSize.Height;
            int width = depthInfo.DepthImageSize.Width;
            //Debug.WriteLine("Depth height; "+ height + " width: "+ width);
            Bitmap image = null;
            if (depthimage != null && _form != null)
            {
                image = ImageProcessing.MakeDepthBitmap(width, height, depthimage);
                //resize the bitmap for display purposes.
                if (image != null)
                {
                    ResizeBilinear filter = new ResizeBilinear(160, 120);
                    _form.SetJDepthPic(filter.Apply(image));
                }

            }
        }

        /// <summary>
        /// A handler for webcam notifications of the kinect for robot J. This handler updates the bitmap
        /// of the robot J in the form.
        /// </summary>
        /// <param name="replaceMessage"></param>
        private void jWebcamReplaceHandler(webcam.Replace replaceMessage)
        {
            var webcamState = replaceMessage.Body;
            try
            {
                if (_form != null && webcamState != null && webcamState.Bitmap != null)
                {
                    Bitmap bitmap = webcamState.Bitmap;
                    _form.SetJKinectPic(bitmap);
                }

            }
            catch (Exception e)
            {
                Debug.WriteLine("Webcam replace handler: " + e.ToString());
            }

        }

        #endregion

        #endregion

        #region Other Handlers

        public IEnumerator<ITask> PatExploreHandler(PatExplore message)
        {
            if (!_runningExplorer)
                _runningExplorer = true;
            else
                yield break;
            
            double previousForwardMultiplier = 0;
            double previousRotationMultiplier = 0;
            double rotationMultiplier = 0;
            double forwardMultiplier = 0;
            double leftWheel = 0.0, rightWheel = 0.0;
            _state.PrevLeftWheelVelocity = -1;
            _state.PrevRightWheelVelocity = -1;

            //Debug.WriteLine("Bumper Status: " + _state.PatBumperPressed);
            while (!_state.PatBumperPressed && !_queueExceededLimit)
            {
                //reads the kinect sensor
                // _state.CurrentKinectReading = DateTime.UtcNow;
                TimeSpan timespan = DateTime.UtcNow - _state.PreviousKinectReading;

                //towait is the amount that it has left waiting.... 
                long toWait = KinectPollingInterval - (long)timespan.TotalMilliseconds;
                if (toWait > 0)
                    yield return TimeoutPort((int)toWait).Receive();

                // get the depth image and get the rotation and forward speed
                yield return _iDepthSensor.Get().Choice(
                    delegate(DepthCamSensorState state)
                    {
                        _state.DepthState = state;
                        // need to save the depth state for furthur computation of the rotational speed if the target is set
                        DepthAnalysis.InterpertDepthImage(state, out forwardMultiplier,
                                                          out rotationMultiplier);
                    },
                    fault => { });



                #region Path Planning and target selection
                // if it is reached the target... set the new target.
                if (_vfh.IsReached())
                {
                    if (_wayPoints.Count != 0)
                        _wayPoints.RemoveAt(0); //remove the previous waypoint
                    if (_wayPoints.Count == 0)
                    {
                        _iCreatePort.CreateDriveDirect(0, 0);
                        _jCreatePort.CreateDriveDirect(0, 0);
                        Console.WriteLine("Total Time:\t"+TimeElapsedInSeconds(_startTime, DateTime.UtcNow));
                        break;
                    }
                    if (_wayPoints.Count != 0)
                        _vfh.SetTarget(_wayPoints[0]);

                }
                if (_jTagged)
                {
                    _numOfObservation = 2;
                }

                if (_numOfObservation >= 2 && _wayPoints.Count != 0)
                {
                    // if we have made distinct observations and there is a target, set the target
                    if (!_vfh.IsTargetSet())
                        _vfh.SetTarget(_wayPoints[0]);

                    double newRotationMultiplier;
                    _vfh.UpdateMap(_state.DepthState, _patAveragedPose, out newRotationMultiplier);
                    if (_histogramForm != null)
                        _histogramForm.HistogramImage(_vfh.GetMap());
                    // if there was no obstacle in the way && !(we could move forward with a slight change of rotational velocity) 
                    if (forwardMultiplier != 0 && !(forwardMultiplier == 1 && Math.Abs(newRotationMultiplier) < 0.1))
                    {
                        rotationMultiplier = newRotationMultiplier;
                    }
                }

                #endregion

                #region Random Exploration
                // queue exceeded limit... make the robot stop... emergency stop
                if (_patActions.Count >= QueueLimit)
                {
                    _queueExceededLimit = true;
                    leftWheel = 0;
                    // mm/sec
                    rightWheel = 0;
                    yield return new IterativeTask(QueueActions); // Queue actions at time t-1 for sampling
                    yield return new IterativeTask(() => GoStraight((int)leftWheel, (int)rightWheel, _iCreatePort, false));
                    Debug.WriteLine("Queue Exceeded limit.... Halting....");

                }

                // if not colliding and (the speed is changed or the time elapsed from the last time we sampled, is greater than sampling interval
                else if (!_state.PatBumperPressed && (Math.Abs(forwardMultiplier - previousForwardMultiplier) > 0.1 ||
                                                Math.Abs(rotationMultiplier - previousRotationMultiplier) > 0.1
                   || (TimeElapsedInSeconds(_state.LastTimeQueued, DateTime.UtcNow) > SamplingInterval)))
                {
                    //Debug.WriteLine("Time elapsed since last time queued: " +
                    //                TimeElapsedInSeconds(_state.LastTimeQueued, DateTime.UtcNow));


                    if (forwardMultiplier == 1)
                    {
                        leftWheel = (int)(((IForwardVelocity * forwardMultiplier) + (RotationDegree * rotationMultiplier)));
                        rightWheel = (int)(((IForwardVelocity * forwardMultiplier) -
                            (RotationDegree * rotationMultiplier))); // mm/sec

                    }
                    else
                    {
                        leftWheel = (int)(((100 * forwardMultiplier) + (75 * rotationMultiplier)));
                        rightWheel = (int)(((100 * forwardMultiplier) -
                            (75 * rotationMultiplier))); // mm/sec
                    }

                    yield return new IterativeTask(QueueActions); // Queue actions at time t-1 for sampling
                    yield return new IterativeTask(() => GoStraight((int)leftWheel, (int)rightWheel, _iCreatePort, true));
                    //Debug.WriteLine("----------------------------------------");
                }
                #endregion
                _state.PreviousKinectReading = DateTime.UtcNow;
                _state.PrevRightWheelVelocity = (int)rightWheel;
                _state.PrevLeftWheelVelocity = (int)leftWheel;
                previousForwardMultiplier = forwardMultiplier;
                previousRotationMultiplier = rotationMultiplier;
            }
            if (_queueExceededLimit)
            {
                while (_patActions.Count > 0)
                {
                    yield return TimeoutPort(1000).Receive();
                }
                _queueExceededLimit = false;
                _runningExplorer = false;
                _mainPort.Post(new PatExplore());
                yield break;
            }
            _runningExplorer = false;
            //Debug.WriteLine("End of Pat Explorer");

        }

        /// <summary>
        /// J's moving strategy
        /// </summary>
        /// <returns></returns>
        public IEnumerator<ITask> MatExploreHandler(MatExplore message)
        {
            Random rand = new Random(DateTime.Now.Millisecond);
            double forwardMultiplier = 0, rotationMultiplier;

            while (!_state.MatBumperPressed)
            {

                // get the depth image and get the rotation and forward speed
                yield return _jDepthSensor.Get().Choice(
                    delegate(DepthCamSensorState state)
                    {
                        // need to save the depth state for furthur computation of the rotational speed if the target is set
                        DepthAnalysis.InterpertDepthImage(state, out forwardMultiplier,
                                                          out rotationMultiplier);
                    },
                    fault => { });

                #region Strategy when j is not tagged
                if (!_jTagged || _pushedLandmark)
                {
                    if (forwardMultiplier != 0)
                    {
                        double num = rand.NextDouble();
                        if (num < 0.99)
                        {
                            // Go straight for JActionTime seconds
                            yield return
                                new IterativeTask(
                                    () =>
                                    GoStraight(JForwardVelocity, JActionTime, _jCreatePort, _jDriveCompleted, false));
                            yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                            yield return new IterativeTask(() => StopRobot(_jCreatePort, _jDriveCompleted));
                            yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);

                        }
                        else if (num < 1)
                        {
                            yield return
                                new IterativeTask(
                                    () => MoveForward(JForwardVelocity, -JForwardVelocity, JActionTime, _jCreatePort, _jDriveCompleted, false));
                            yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                        }
                        else
                        {
                            yield return
                                new IterativeTask(
                                    () => MoveForward(-JForwardVelocity, JForwardVelocity, JActionTime, _jCreatePort, _jDriveCompleted, false));
                            yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                        }
                    }

                    else
                    {
                        //TODO: Change the behavior of the j to rotate for JAction time isntead of 90 degree when bumped into something
                        double num = rand.NextDouble();
                        if (num < 0.5)
                        {
                            yield return
                                new IterativeTask(
                                    () => RotateRight(90, JForwardVelocity, _jCreatePort, _jDriveCompleted, false));
                            yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                        }
                        else
                        {
                            yield return
                                new IterativeTask(
                                    () => RotateRight(90, JForwardVelocity, _jCreatePort, _jDriveCompleted, false));
                            yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                        }
                    }
                }//end of if(!_jTagged)
                #endregion

                #region when j is tagged
                else if (!_facedTheObject)
                {
                    //find closest landmark to the robot
                    Landmark closestLandmark = Landmark.ClosestLandmarkByDistance(_mapLandmarks,
                                                                                  _state.MatPose);
                    //Debug.WriteLine("Closest Landmark: " + closestLandmark);
                    //Face toward the object.
                    double thetaToRotate = FaceTowardTheObject(_state.MatPose, closestLandmark.Center);
                    Debug.WriteLine("Theta to rotate: " + thetaToRotate);
                    if (thetaToRotate > 0)
                    {
                        yield return new IterativeTask(
                            () => RotateRight(thetaToRotate, JPushVelocity, _jCreatePort, _jDriveCompleted, false));
                        yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                    }
                    else if (thetaToRotate < 0)
                    {
                        yield return new IterativeTask(
                            () => RotateLeft(Math.Abs(thetaToRotate), JPushVelocity, _jCreatePort, _jDriveCompleted, false));
                        yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                    }
                    _facedTheObject = true;
                }
                // if it has not pushed the landmark move toward the landmark
                else if (_facedTheObject && !_reachedLandmark)
                {
                    // GoStraight
                    yield return
                        new IterativeTask(
                            () =>
                            GoStraight(JPushVelocity, JPushVelocity, _jCreatePort, false));


                }
                // if the bumper is pressed for pushhing the landmark.... stop and move forward for 2 seconds
                else if (_reachedLandmark)
                {
                    
                    //Debug.WriteLine("J Reached landmark... time to push");
                    yield return new IterativeTask(() => StopRobot(_jCreatePort, _jDriveCompleted));
                    yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                    yield return TimeoutPort(1000).Receive();
                    yield return
                        new IterativeTask(() => GoStraight(JPushVelocity, 4000, _jCreatePort, _jDriveCompleted, false));
                    yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
                    _pushedLandmark = true;
                    Console.WriteLine("Pushed\t" + DateTime.UtcNow.TimeOfDay);
                }

                #endregion
            }
        }

        ///// <summary>
        ///// Updates the map in every few seconds
        ///// </summary>
        ///// <returns></returns>
        public void UpdateMapHandler()
        {

            if (_state.PatPose != null)
            // it gets updated either in querySimulation handler or update odometry handler
            {
                _iMap.UpdateMap(_state.PatPose, _state.MatPose, _patParticles);

            }
            else
                _iMap.UpdateMap(_patParticles);

            if (_state.PatPose != null && _patParticles != null && _mostLikelyJParticle!= null)
            {
                _jMap.UpdateMapWithNestedParticles(_state.MatPose, _mostLikelyJParticle.Pose, _patParticles);
            }
            else
            {
                _jMap.UpdateMapWithNestedParticles(_patParticles);
            }


            if (_form != null)
            {
                _form.SetBoydPicForI(_iMap.GetBitmap());
                _form.SetBoydPicFoJ(_jMap.GetBitmap());
                _form.SetPatClusteredPose(_clusteredPose);
                _form.SetPatAveragePose(_patAveragedPose);
                if (_wayPoints.Count !=0)
                 _form.SetWayPoint((Point) _wayPoints[0]);

            }
            //yield return TimeoutPort(UpdateMapInterval).Receive();
        }

        #endregion

        #region Help Functions


        /// <summary>
        /// Stops the drive time and Queue the actions, such as transitional and rotational velocity,
        /// time it took to perform the action , and the picture taken at the specified time
        /// from the scene for propagation and observation purposes
        /// </summary>
        private IEnumerator<ITask> QueueActions()
        {

            double vleft = _state.PrevLeftWheelVelocity;
            double vright = _state.PrevRightWheelVelocity;

            if (vright != -1 && vleft != -1)
            {
                if (_state.KinectBitmap != null)
                    lock (_state.KinectBitmap)
                        _state.PatWebcamImage = (Bitmap)_state.KinectBitmap.Clone();

                double xVelocity = Velocity(vleft, vright) / 1000.0; //m/sec
                double xOmega = Omega(vright, vleft);

                var motion = new Motions(xVelocity, xOmega);
                //Debug.WriteLine("\t\t"+ motion.ToString()+ " time: "+ timeElapsed);


                if (_state.DepthState == null)
                {
                    yield return _iDepthSensor.Get().Choice(
                        delegate(DepthCamSensorState state)
                        {
                            _state.DepthState = state;

                        },
                        fault => { });
                }

                var depth = (DepthCamSensorState)_state.DepthState.Clone();
                // process the image for landmnarks here...
                List<IdentifiedLandmark> identifiedLandmarks = null;
                Bitmap bmp = ImageProcessing.AnalyzeEnvironmet(_state.PatWebcamImage, _state.DepthState,
                                                                _iBeleivedLandmarks,
                                                                out identifiedLandmarks);
                // process the image for the robot here.
                double robotdepth = 0, robotangle = 0;
                CustomisedColor robotColor;
                bool identifiedRobot = ImageProcessing.FilterRobotInSimulator((Bitmap)_state.PatWebcamImage.Clone(),
                                                                              _state.DepthState,
                                                                              out robotdepth, out robotangle, out robotColor);
                
                _state.StopDriveTime = DateTime.UtcNow;
                _state.PreviousKinectReading = DateTime.UtcNow;
                _state.LastTimeQueued = DateTime.UtcNow;
                double timeElapsed = TimeElapsedInSeconds(_state.StartDriveTime, _state.StopDriveTime);
                //if (timeElapsed > 1000)
                   // timeElapsed = 110*timeElapsed/100;


                var p = new Parameter(motion, timeElapsed, depth, _state.PatPose);
                p.IdentifiedLandmarks = identifiedLandmarks;
                p.IdentifiedRobot = identifiedRobot;
                p.RobotAngle = robotangle;
                p.RobotDepth = robotdepth;
                p.RobotColor = robotColor;
                p.TimeQueued = DateTime.UtcNow;
                
                _patActions.Enqueue(p);
                ParticleFilter.next = true;

                yield break;

            }
        }


        /// <summary>
        /// Queue the actions, such as transitional and rotational velocity,
        /// time it took to perform the action , and the picture taken at the specified time
        /// from the scene for propagation and observation purposes
        /// <param name="time">time in seconds</param>
        /// </summary>
        private IEnumerator<ITask> QueueActions(double time)
        {

            // Debug.WriteLine("Queuing the following motion:");
            double vleft, vright, timeElapsed;
            lock (_state) // lock the state so other threads won't change the state
            {
                vleft = _state.PrevLeftWheelVelocity;
                vright = _state.PrevRightWheelVelocity;
                timeElapsed = time; // in seconds
                _state.LastTimeQueued = DateTime.UtcNow;
            }
            if (vright != -1 && vleft != -1)
            {
                lock (_state.KinectBitmap)
                    _state.PatWebcamImage = (Bitmap)_state.KinectBitmap.Clone();
                double xVelocity = Velocity(vleft, vright) / 1000.0; //m/sec
                double xOmega = Omega(vright, vleft);
                var motion = new Motions(xVelocity, xOmega);

                if (_state.DepthState == null)
                {
                    yield return _iDepthSensor.Get().Choice(
                    delegate(DepthCamSensorState state)
                    {
                        _state.DepthState = state;

                    },
                    fault => { });
                }
                DepthCamSensorState depth = (DepthCamSensorState)_state.DepthState.Clone();
                // yield return new IterativeTask(QueryRobotPosition);
                // process the image for landmnarks here...
                List<IdentifiedLandmark> identifiedLandmarks = null;
                Bitmap bmp = ImageProcessing.AnalyzeEnvironmet(_state.PatWebcamImage, _state.DepthState,
                                                                _iBeleivedLandmarks,
                                                                out identifiedLandmarks);


                // process the image for the robot here.
                double robotdepth = 0, robotangle = 0;
                CustomisedColor robotColor;
                bool identifiedRobot = ImageProcessing.FilterRobotInSimulator((Bitmap)_state.PatWebcamImage.Clone(),
                                                                              _state.DepthState,
                                                                              out robotdepth, out robotangle, out robotColor);
                
                _state.StopDriveTime = DateTime.UtcNow;
                _state.PreviousKinectReading = DateTime.UtcNow;
                _state.LastTimeQueued = DateTime.UtcNow;
                var p = new Parameter(motion, timeElapsed, depth, _state.PatPose);
                p.IdentifiedLandmarks = identifiedLandmarks;
                p.IdentifiedRobot = identifiedRobot;
                p.RobotAngle = robotangle;
                p.RobotDepth = robotdepth;
                p.RobotColor = robotColor;
                p.TimeQueued = DateTime.UtcNow;
                _patActions.Enqueue(p);
                ParticleFilter.next = true;

                yield break;
            }
        }
        /// <summary>
        /// Sample the particles if there is any action remained in the queue.
        /// This function runs on a different thread than the main thread
        /// </summary>
        private IEnumerator<ITask> SampleParticles()//Sample message)
        {
            
            while (true)
            {
                // lock (_patParticles)
                {
                    if (_patActions != null && _patActions.Count != 0)
                    {
                        while (_patActions.Count != 0)
                        {
                            Debug.WriteLine(_patActions.Count + " actions in the queue");
                            Parameter parameter = _patActions.Dequeue();
                            ParticleFilter.next = false;

                            _state.ObservedColor = null;
                            _state.ObservedAngle = 0;
                            _state.ObservedDepth = 0;

                            #region check for image/ filters landmarks and robots
                            if (parameter.DepthState != null && parameter.Time > 1)
                            {

                                #region Postive Landmark Observation
                                if (parameter.IdentifiedLandmarks != null && parameter.IdentifiedLandmarks.Count != 0)
                                {
                                    _numOfObservation = DistinctObservation(parameter.IdentifiedLandmarks);
                                    _state.ObservedAngle = parameter.IdentifiedLandmarks[0].Angle;
                                    _state.ObservedColor = parameter.IdentifiedLandmarks[0].ObservedColor;
                                    _state.ObservedDepth = parameter.IdentifiedLandmarks[0].Range;

                                    String[] name = parameter.IdentifiedLandmarks[0].Landmark.Name.Split('_');
                                   // if (_jTagged)
                                    {
                                        Console.WriteLine( name[1]);
                                        //Console.WriteLine("Depth: "+_state.ObservedDepth);
                                    }
                                    // _observedLandmarkName = name[1];
                                }
                                #endregion


                                #region Negative Landmark Observation
                                else if (parameter.IdentifiedLandmarks == null || parameter.IdentifiedLandmarks.Count == 0)
                                {
                                    _state.ObservedAngle = 5;
                                    _state.ObservedColor = new CustomisedColor(0, 0, 0);
                                    _state.ObservedDepth = 5000;
                                    //_observedLandmarkName = String.Empty;
                                }
                                #endregion

                                #region Robot Identification


                                if (parameter.IdentifiedRobot && parameter.RobotDepth != 0)
                                {

                                    if (parameter.RobotDepth < TaggedDistance && !_jTagged)
                                    {
                                        _jTagged = true;
                                        _taggedTime = DateTime.UtcNow;
                                        Console.WriteLine("Tagged\t"+_taggedTime.TimeOfDay);
                                       
                                        MarkNestedParticlesAsTagged();
                                    }

                                }
                                #endregion


                            }//end of if

                            else if (parameter.DepthState == null)
                            {
                                Debug.WriteLine("Depth is null");

                            }
                            else if (parameter.Time < 1)
                            {
                                Debug.WriteLine("Time is too short for processing the images...");
                                _state.ObservedColor = new CustomisedColor(0, 0, 0);
                            }

                            #endregion

                            int lastindex = 0;
                            ParticleFilter.jPose = _state.MatPose;
                            DateTime a = DateTime.UtcNow;
                            _mostLikelyParticle = ParticleFilter.Propagate(_patParticles, parameter, JForwardVelocity,
                                                                           _iBeleivedLandmarks,
                                                                           _state.ObservedDepth, _state.ObservedAngle,
                                                                           _state.ObservedColor, Level,
                                                                           out lastindex);
                            
                            _meanError = Particle.MeanError(_patParticles, parameter.RealRobotPose);
                            Debug.WriteLine("Propagation Time: "+TimeElapsedInSeconds(a,DateTime.UtcNow));
                            Console.WriteLine(_meanError + "\t"+ parameter.TimeQueued.TimeOfDay+"\t" +(((lastindex) * 100.0) / _patParticles.Count)+ "%");
                            
                            //NOTE: comment out the following if you would like to localize based on the real map and not an updated map
                            UpdateHypothesisedMap();

                             
                            //------------------------------------------------------------------------------------------------------------
                            _patAveragedPose = Particle.AveragedPose(_patParticles);
                            _cluster.PerformClustering(_patParticles);
                            _clusteredPose = _cluster.MeanOfMostDensedCluster();
                            UpdateMapHandler();
                           // Debug.WriteLine("Most likely: "+ _mostLikelyParticle.Pose);
                            Particle l = ParticleFilter.MostLikelyParticle(_patParticles);
                           // Debug.WriteLine(l.Pose);
                            _mostLikelyJParticle = ParticleFilter.MostLikelyParticle(l.NestedParticles);
                            
                           
                        }//end of while

                    }// end of if
                }
                // sample every few seconds
                yield return TimeoutPort(PollQueueForSampling).Receive();
            }//end of while
        }

        /// <summary>
        ///  Returns the number of distinct observation, the robot has made upto now
        /// </summary>
        /// <param name="identifiedLandmarks"></param>
        /// <returns></returns>
        /// <exception cref="ArgumentNullException"><paramref name="identifiedLandmarks" /> is <c>null</c>.</exception>
        private int DistinctObservation(List<IdentifiedLandmark> identifiedLandmarks)
        {
            if (identifiedLandmarks == null)
                throw new ArgumentNullException("identifiedLandmarks", "parameter is null");

            // just check the first item of identified landmark since all identified landmarks
            // made identified in a single observation, have the same color. 
            bool exists = false;

            for (int i = 0; i < _observedEntityColor.Count; i++)
                if (identifiedLandmarks[0].Landmark.EntityColor.Hue == _observedEntityColor[i].Hue
                    && identifiedLandmarks[0].Landmark.EntityColor.Saturation == _observedEntityColor[i].Saturation
                    && identifiedLandmarks[0].Landmark.EntityColor.Luminance == _observedEntityColor[i].Luminance)
                {
                    exists = true;
                    break;
                }
            // if the color doesn't exist add it to the list of the color of landmarks that are observed
            if (!exists)
                _observedEntityColor.Add(identifiedLandmarks[0].Landmark.EntityColor);
            return _observedEntityColor.Count;
        }


        /// <summary>
        /// The angle (degree) in which the robot has to turn in order to face the object
        /// </summary>
        /// <param name="pose">Robot's pose</param>
        /// <param name="obj">object of interest</param>
        /// <returns></returns>
        private double FaceTowardTheObject(RobotPose pose, Point obj)
        {
            double theta = Math.Atan2(-obj.X + pose.X, -obj.Z + pose.Z);     //?between the object and the robot in radian
            theta = UnitConversion.RadianToDegree(UnitConversion.PutThetaInBound(theta)); // in degree
            theta = pose.Theta - theta; // The amount of angle in which the robot has to turn (in degree)
            return theta;
        }

        /// <summary>
        /// Mark all nested particle as tagged.
        /// </summary>
        public void MarkNestedParticlesAsTagged()
        {
            for (int i = 0; i < _patParticles.Count; i++)
            {
                Particle p = _patParticles[i];
                for (int j = 0; j < p.NestedParticles.Count; j++)
                {
                    p.NestedParticles[j].IsTagged = true;
                    p.NestedParticles[j].LeftoverMotion = null;
                    p.NestedParticles[j].LeftoverTime = 0;
                }
            }
        }
        /// <summary>
        /// Updating the map based on j's particle that have pushed the landmark.
        /// </summary>
        public void UpdateHypothesisedMap()
        {
            if (!_jTagged)
                return;
            
            // for evert particle of robot i
            foreach (Particle particle in _patParticles)
            {
                foreach (Particle nested in particle.NestedParticles)
                {
                    int index = nested.LandmarkIndex; // get the index of the landmark in which it is supposed to push
                    if ( index>=0  && nested.LandmarkToPush != null && nested.IsDone && !nested.IsMapUpdated )
                    {
                        //Console.WriteLine(i + " " + j + " pushed ");
                        landmarkPoses[index].X += nested.LandmarkToPush.Center.X; // update the x of the landmark in the map
                        landmarkPoses[index].Z += nested.LandmarkToPush.Center.Z;
                        count[index]++;
                        nested.IsMapUpdated = true;
                    }
                        
                }
            }
            
            for (int i = 0; i < landmarkPoses.Length; i++)
            {
                
                if (count[i] != 0) // the landmark at index i is pushed by some particle then...
                {
                    
                    _iMap.ClearDynamicLandmark(_iBeleivedLandmarks[i]);
                    double x  = landmarkPoses[i].X / count[i];
                    double z = landmarkPoses[i].Z / count[i];
                    _iBeleivedLandmarks[i].Center.X = x;
                    _iBeleivedLandmarks[i].Center.Z = z;
                    _iBeleivedLandmarks[i].RecalculateCornerPoints();
                   
                    //if (_iBeleivedLandmarks[i].Name.Contains("blue_1"))
                    //{
                    //    _iBeleivedLandmarks[i].Center.X = bluelm.X;
                    //    _iBeleivedLandmarks[i].Center.Z= bluelm.Z;
                    //    _iBeleivedLandmarks[i].RecalculateCornerPoints();
                        
                    //}

                    _iMap.DrawDynamicLandmark(_iBeleivedLandmarks[i]);
                  
                }
               
           
            }

        }//end of update_i_map

        public void PrintQueue()
        {
            lock (_patActions)
            {

                var a = _patActions.GetEnumerator();

                while (a.MoveNext())
                {
                    Debug.WriteLine(a.Current.Action.ToString() + " " + a.Current.Time);
                }
            }

        }
        #endregion

        /*****************************************************************************************/

        #region Subscribtion Functions

        /// <summary>
        /// Subscribing to the depthcam service to receive the depth information (No need for polling)
        /// </summary>
        /// <returns>ITask to be scheduled for the dispatcher</returns>
        public IEnumerator<ITask> SubscribeToDepthcam()
        {
            yield return Arbiter.Choice(_iDepthSensor.Subscribe(_iDepthNotifyPort),
                                        s => LogVerbose(TraceLevel.Verbose, "Subscribing to Depthcam Service for iRobot: Success"),
                                        f => LogVerbose(TraceLevel.Error, "Subscribing to Depthcam Service for iRobot: Failed: " + f.ToString()));
            yield return Arbiter.Choice(_jDepthSensor.Subscribe(_jDepthNotifyPort),
                                        s => LogVerbose(TraceLevel.Verbose, "Subscribing to Depthcam Service for jRobot: Success"),
                                        f => LogVerbose(TraceLevel.Error, "Subscribing to Depthcam Service for jRobot: Failed: " + f.ToString()));
            yield break;
        }

        /// <summary>
        /// Subscribing to the webcam service in order to receive the images (No need for polling)
        /// </summary>
        /// <returns>ITask to be scheduled for dispatcher</returns>
        public IEnumerator<ITask> SubscribeToWebcam()
        {
            yield return Arbiter.Choice(
                _iWebcamPort.Subscribe(_iWebcamNotify),
                                        s => LogVerbose(TraceLevel.Verbose, "Subscribing to Webcam Service for iRobot: Success"),
                                        f => LogVerbose(TraceLevel.Error, "Subscribing to Webcam Service for iRobot: Failed: " + f.ToString()));

            yield return Arbiter.Choice(
                _jWebcamPort.Subscribe(_jWebcamNotify),
                                        s => LogVerbose(TraceLevel.Verbose, "Subscribing to Webcam Service for jRobot: Success"),
                                        f => LogVerbose(TraceLevel.Error, "Subscribing to Webcam Service for jRobot: Failed: " + f.ToString()));
            yield break;

        }

        /// <summary>
        /// Subscribing to lite service and activating Handlers to receive notifications
        /// </summary>
        /// <returns>ITask to be scheduled for dispatcher</returns>
        public IEnumerator<ITask> SubscribeToLite()
        {
            yield return Arbiter.Choice(
                _iCreatePort.Subscribe(_iCreateNotify),
                sucess => Debug.WriteLine("Subscribing to Lite Service for iRobot: Success"),
                failure => Debug.WriteLine("Subscribing to Lite Service for iRobot: Failed")

                );

            yield return Arbiter.Choice(
                _jCreatePort.Subscribe(_jCreateNotify),
                sucess => Debug.WriteLine("Subscribing to Lite Service for jRobot: Success"),
                failure => Debug.WriteLine("Subscribing to Lite Service for jRobot: Failed")
                );

            yield break;
        }


        #endregion

        /*****************************************************************************************/

        #region Forms
        /// <summary>
        /// Making the Map Form
        /// </summary>
        /// <returns>true if successful and false otherwise</returns>
        private bool MakeMapForm()
        {
            Debug.WriteLine("-----------------------------------");
            Debug.WriteLine("Creating Map Form");
            Fault fault = null;
            RunForm runform = new RunForm(CreateMapForm); // CreateForm is a method to create a form object.
            //Console.WriteLine("Creating the form....");
            WinFormsServicePort.Post(runform);
            Arbiter.Choice(runform.pResult,
                    success => Debug.WriteLine("\tMap form created sucessfully"),
                    delegate(Exception e)
                    {
                        fault = Fault.FromException(e);
                    });


            if (fault != null)
            {
                Debug.WriteLine("\tFailed to create Map window " + fault);
                Debug.WriteLine("-----------------------------------");
                return false;
            }
            Debug.WriteLine("-----------------------------------");
            return true;
        }// end of makeForm

        /// <summary>
        /// Making the Histogram Form
        /// </summary>
        /// <returns>true if successful and false otherwise</returns>
        private bool MakeHistogramForm()
        {
            Debug.WriteLine("-----------------------------------");
            Debug.WriteLine("Creating Historam Form");
            Fault fault = null;
            RunForm runform = new RunForm(CreateHistogramForm); // CreateForm is a method to create a form object.
            //Console.WriteLine("Creating the form....");
            WinFormsServicePort.Post(runform);
            Arbiter.Choice(runform.pResult,
                    success => Debug.WriteLine("\tHistogram form created sucessfully"),
                    delegate(Exception e)
                    {
                        fault = Fault.FromException(e);
                    });


            if (fault != null)
            {
                Debug.WriteLine("\tFailed to create Histogram window " + fault);
                Debug.WriteLine("-----------------------------------");
                return false;
            }
            Debug.WriteLine("-----------------------------------");
            return true;
        }// end of makeForm

        /// <summary>
        /// Constructor to create the map form
        /// </summary>
        /// <returns>new constructed map form</returns>
        private Form CreateMapForm()
        {
            _form = new ImageForm(_mainPort, _iKinectPort, _iCreatePort, _jKinectPort, _jCreatePort);
            return _form;
        }
        /// <summary>
        /// Constructor to create the form
        /// </summary>
        /// <returns>new constructed form</returns>
        private Form CreateHistogramForm()
        {
            _histogramForm = new HistogramForm();
            return _histogramForm;
        }
        #endregion

        /*****************************************************************************************/

        #region Query Simulator
        /// <summary>
        /// This will query the location of the robot in the simulator every second for displaying purposes
        /// </summary>
        /// <returns>Itask to be scheduled for dispatcher</returns>
        public IEnumerator<ITask> QueryRobotPosition()
        {
            bool queryStatus = true;
            while (queryStatus)
            {

                VisualEntity tmp1 = new VisualEntity();
                VisualEntity tmp2 = new VisualEntity();
                tmp1.State.Name = "uboo";
                tmp2.State.Name = "jCreate";
                VisualEntity tmp3 = new VisualEntity();
                tmp3.State.Name = "landmark_blue_1";
                // query the simulator for the robot's location
                yield return Arbiter.Choice(SimulationEngine.GlobalInstancePort.Query(tmp1),
                                            response =>
                                            {
                                                // isSimulated = true;
                                                if (_state.PatPose == null)
                                                    _state.PatPose = new RobotPose();
                                                Microsoft.Robotics.PhysicalModel.Pose p = response.Entity.State.Pose;
                                                _state.PatPose.X = p.Position.X;
                                                _state.PatPose.Z = p.Position.Z;
                                                _state.PatPose.Theta = (float)(response.Entity.RotationAngles.Y);
                                                _form.SetPatRealPose(_state.PatPose);
                                            },
                                            f =>
                                            {
                                                Debug.WriteLine("Query simulation failed");
                                                queryStatus = false;
                                            }
                    );
                yield return Arbiter.Choice(SimulationEngine.GlobalInstancePort.Query(tmp2),
                                           response =>
                                           {
                                               // isSimulated = true;
                                               if (_state.MatPose == null)
                                                   _state.MatPose = new RobotPose();
                                               Microsoft.Robotics.PhysicalModel.Pose p = response.Entity.State.Pose;
                                               _state.MatPose.X = p.Position.X;
                                               _state.MatPose.Z = p.Position.Z;
                                               _state.MatPose.Theta = (float)(response.Entity.RotationAngles.Y);

                                           },
                                           f =>
                                           {
                                               Debug.WriteLine("Query simulation failed");
                                               queryStatus = false;
                                           }
                   );
                yield return Arbiter.Choice(SimulationEngine.GlobalInstancePort.Query(tmp3),
                                          response =>
                                          {
                                              
                                              Microsoft.Robotics.PhysicalModel.Pose p = response.Entity.State.Pose;
                                              bluelm.X = p.Position.X;
                                              bluelm.Z = p.Position.Z;

                                          },
                                          f =>
                                          {
                                              Debug.WriteLine("Query simulation failed");
                                              queryStatus = false;
                                          }
                  );
                yield return Arbiter.Receive(false, TimeoutPort(SimulationQueryInterval), delegate { });
            }
            Debug.WriteLine("....No Simulator Exist ....");
        }
        #endregion

        /*****************************************************************************************/
        public IEnumerator<ITask> SimulationResetHandler(SimulationReset reset)
        {



            Microsoft.Robotics.Simulation.Proxy.SimulationState simstate = null;
            Fault error = null;
            // Read file
            mount.MountServiceOperations s = new MountServiceOperations();
            mount.MountServiceOperations mountService = ServiceForwarder<mount.MountServiceOperations>("..\\simulationengine");
            var get = new DsspDefaultGet();
            mountService.Post(get);

            yield return Arbiter.Choice(get.ResponsePort, (fault) => error = fault,
              (success) =>
              {
                  simstate = (Microsoft.Robotics.Simulation.Proxy.SimulationState)success;
                  Debug.WriteLine("Success");
              });

            if (simstate == null)
                Debug.WriteLine("Simstate is null");
            if (error != null)
            {
                LogError(error.ToString());
                reset.ResponsePort.Post(error);
                yield break;
            }

            // Pause
            simstate.Pause = true;
            yield return Arbiter.Choice(_simEngine.Replace(simstate), (success) => { }, LogError);

            VisualEntity[] entities = new VisualEntity[simstate.SerializedEntities.XmlNodes.Count];
            int i = 0;

            foreach (XmlElement element in simstate.SerializedEntities.XmlNodes)
            {

                Deserialize request = new Deserialize(element.OuterXml);
                SerializerPort.Post(request);

                VisualEntity entity = null;

                yield return Arbiter.Choice(
                  request.ResultPort,
                  (success) => entity = (VisualEntity)DssTypeHelper.TransformFromProxy(success.Instance),
                  LogError);

                entities[i] = entity;
                ++i;
            }

            //VisualEntity tmp = new VisualEntity();
            //tmp.State.Name = "uboo";
            //yield return Arbiter.Choice(
            //    SimulationEngine.GlobalInstancePort.Query(tmp),
            //                                          success => { Debug.WriteLine("sdda"); },
            //                                          fault => Debug.WriteLine("dasda"));

            //tmp.EntityState.Pose = new Pose(new Vector3(9f, 0.1f, -3));

            //UpdateSimulationEntity update = new UpdateSimulationEntity(tmp);
            //SimulationEngine.GlobalInstancePort.Post(update);
            //yield return Arbiter.Choice(update.ResponsePort,
            //                             success => Debug.WriteLine("yey"),
            //                             fauilure => Debug.WriteLine("shit"));
            for (i = 0; i < entities.Length; ++i)
            {
                if (entities[i] == null)
                    break;
                VisualEntity entity = entities[i];

                //if (entity.EntityState.Name.Equals("jKinectEntity"))
                //{
                //    _jDepthSensor.Post(new DsspDefaultDrop());
                //    //DeleteSimulationEntity delete = new DeleteSimulationEntity(entity);
                //    //SimulationEngine.GlobalInstancePort.Post(delete);

                //    //yield return Arbiter.Choice(
                //    //  delete.ResponsePort,
                //    //  (success) => { }, LogError);

                //}


                if (entity.EntityState.Name.Equals("jCreate"))//|| entity.EntityState.Name.Equals("KinectEntity"))
                {
                    _jDepthSensor.Post(new DsspDefaultDrop());
                    //DeleteSimulationEntity delete = new DeleteSimulationEntity (entity);
                    //SimulationEngine.GlobalInstancePort.Post(delete);

                    //yield return Arbiter.Choice(
                    //  delete.ResponsePort,
                    //  (success) => { }, LogError);

                    int j, k = 0;
                    for (j = 0; j < entity.ChildCount; j++)
                    {
                        VisualEntity child = entities[j + i + 1];

                        entities[i].InsertEntityGlobal(child);
                        entities[i].InsertEntity(child);

                        if (child.ChildCount != 0)
                        {
                            Debug.WriteLine("WAAAA");
                            for (k = 0; k < child.ChildCount; k++)
                            {
                                VisualEntity nested = entities[j + k + i + 2];
                                child.InsertEntity(nested);
                                //SimulationEngine.GlobalInstance.RefreshEntity(nested);

                            }

                        }

                        Debug.WriteLine("child entities: " + child.EntityState.Name);
                    }
                    entity.EntityState.Pose = new Pose(new Vector3(9, 0.0001f, -3));

                    UpdateSimulationEntity u = new UpdateSimulationEntity(entity);

                    SimulationEngine.GlobalInstancePort.Post(u);

                    yield return Arbiter.Choice(
                      u.ResponsePort,
                      (success) => { Debug.WriteLine("halelooya"); }, LogError);

                    //InsertSimulationEntity insert = new InsertSimulationEntity(entity);
                    //SimulationEngine.GlobalInstancePort.Post(insert);

                    //yield return Arbiter.Choice(
                    //  insert.ResponsePort,
                    //  (success) => { Debug.WriteLine("halelooya"); }, LogError);


                    //yield return Arbiter.Choice(_jDepthSensor.DsspDefaultLookup(),
                    //                          ssss => Debug.WriteLine("adasda"),
                    //                          ffff => Debug.WriteLine("fualt"+ ffff));

                    i = i + j + k;

                }

                else if (entity is SkyEntity || entity is LightSourceEntity || entity is HeightFieldEntity || entity is CameraEntity || entity is KinectEntity || entity is IRobotCreate || entity is KinectCameraEntity)
                {
                    continue;
                }
                else if (entity.State.Name.Equals("jKinectSupport") || entity.State.Name.Equals("KinectSupport"))
                    continue;

                //else
                //    SimulationEngine.GlobalInstancePort.Update(entity);

            }
            Debug.WriteLine("Replacing the simulation...");
            simstate.Pause = false;
            yield return Arbiter.Choice(_simEngine.Replace(simstate), (success) => Debug.WriteLine("Finished replacing the state"),
                                        (fault) => Debug.WriteLine("Error in replacing the simulation state" + " " + fault.ToString()));
        }




        #region Test Handlers
        public IEnumerator<ITask> GoFroward(PatExplore patExplore)
        {
            yield return new IterativeTask(() => GoStraight(IForwardVelocity, 5000.0, _iCreatePort, _iDriveCompleted, true));
            yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
            yield return new IterativeTask(() => StopRobot(_iCreatePort, _iDriveCompleted));
            yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
            //----------------------------------------------------------------------------/
            // rotate using rotateRight function. this will queue the action so no need to 
            // call QueueAction specifically here.
            yield return new IterativeTask(() => RotateRight(90, 100, _iCreatePort, _iDriveCompleted, true));
            yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
            //----------------------------------------------------------------------------/
            yield return new IterativeTask(() => GoStraight(IForwardVelocity, 1000.0, _iCreatePort, _iDriveCompleted, true));
            yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
            yield return new IterativeTask(() => StopRobot(_iCreatePort, _iDriveCompleted));
            yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);

            yield return new IterativeTask(() => RotateLeft(90, 100, _iCreatePort, _iDriveCompleted, true));
            yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);

            yield return new IterativeTask(() => GoStraight(IForwardVelocity, 3000.0, _iCreatePort, _iDriveCompleted, true));
            yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
            yield return new IterativeTask(() => StopRobot(_iCreatePort, _iDriveCompleted));
            yield return Arbiter.Receive(false, _iDriveCompleted, EmptyHandler);
        }

        public IEnumerator<ITask> FaceTowardObjectTestHandler(MatExplore matExplore)
        {
            Point lm1 = new Point(13, 6.14);
            double thetaToRotate = FaceTowardTheObject(_state.MatPose, lm1);
            if (thetaToRotate > 0)
            {
                yield return new IterativeTask(
                    () => RotateRight(thetaToRotate, 200, _jCreatePort, _jDriveCompleted, false));
                yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
            }
            else if (thetaToRotate < 0)
            {
                yield return new IterativeTask(
                    () => RotateLeft(Math.Abs(thetaToRotate), 200, _jCreatePort, _jDriveCompleted, false));
                yield return Arbiter.Receive(false, _jDriveCompleted, EmptyHandler);
            }

        }
        #endregion

    }
}


